#ifndef MOVE_YOUBOT_BASE__CLIENT_H_
#define MOVE_YOUBOT_BASE__CLIENT_H_

#define TESTING 0

#include "youbot_relay/MoveYoubotBaseAction.h"

#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/simple_client_goal_state.h>

#include "ros/ros.h"

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>

#include <tf/transform_datatypes.h>

#define CLIENT_NODE "move_youbot_base_client"
#define CLIENT_ACTION "move_youbot_base"
#define DEFAULT_TIMEOUT ros::Duration(10, 0)


namespace move_youbot_base
{
	class MoveYoubotBaseClient
	{
	public:
	  typedef actionlib::SimpleActionClient<youbot_relay::MoveYoubotBaseAction> Client;

	  MoveYoubotBaseClient(int argc, char** argv);
	  bool move( double x=0, double y=0, double theta=0, int velocity=100, ros::Duration timeout=ros::Duration(0, 0));
	  bool move( youbot_relay::MoveYoubotBaseGoal g, ros::Duration timeout=ros::Duration(0, 0) );
	  bool move( ros::Duration timeout=ros::Duration(0, 0) );
	  bool moveSynchronous( double x_meters=0, double y_meters=0, double theta_degree=0, int velocity_percent=100);
	  bool getCurrentPosition( double *x, double *y, double *theta );
	  actionlib::SimpleClientGoalState getState();
	  bool stop();

	private:
	  Client *client;
	  youbot_relay::MoveYoubotBaseGoal goal;
	  double pos_x, pos_y, pos_theta;

	  void setGoal( youbot_relay::MoveYoubotBaseGoal g);
	  void setGoal(double x_meters=0, double y_meters=0, double theta_degree=0, int velocity_percent=100);
    bool sendGoal();
    actionlib::SimpleClientGoalState sendGoalForResult( ros::Duration timeout = DEFAULT_TIMEOUT);

	};
}


#endif
